/*
 * Copyright 2018 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "cartographer/mapping/2d/grid_2d.h"

namespace cartographer {
	namespace mapping {
		namespace {

			float MinCorrespondenceCostFromProto(const proto::Grid2D& proto) {
				if (proto.min_correspondence_cost() == 0.f &&
					proto.max_correspondence_cost() == 0.f) {
					LOG(WARNING) << "proto::Grid2D: min_correspondence_cost "
						"is initialized with 0 indicating an older version of the "
						"protobuf format. Loading default values.";
					return kMinCorrespondenceCost;
				}
				else {
					return proto.min_correspondence_cost();
				}
			}

			float MaxCorrespondenceCostFromProto(const proto::Grid2D& proto) {
				if (proto.min_correspondence_cost() == 0.f &&
					proto.max_correspondence_cost() == 0.f) {
					LOG(WARNING) << "proto::Grid2D: max_correspondence_cost "
						"is initialized with 0 indicating an older version of the "
						"protobuf format. Loading default values.";
					return kMaxCorrespondenceCost;
				}
				else {
					return proto.max_correspondence_cost();
				}
			}
		}  // namespace

		proto::GridOptions2D CreateGridOptions2D(
			common::LuaParameterDictionary* const parameter_dictionary) {
			proto::GridOptions2D options;
			const std::string grid_type_string =
				parameter_dictionary->GetString("grid_type");
			proto::GridOptions2D_GridType grid_type;
			CHECK(proto::GridOptions2D_GridType_Parse(grid_type_string, &grid_type))
				<< "Unknown GridOptions2D_GridType kind: " << grid_type_string;
			options.set_grid_type(grid_type);
			options.set_resolution(parameter_dictionary->GetDouble("resolution"));
			return options;
		}

		Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
			float max_correspondence_cost,
			ValueConversionTables* conversion_tables)
			: limits_(limits),
			correspondence_cost_cells_(
				limits_.cell_limits().num_x_cells* limits_.cell_limits().num_y_cells,
				kUnknownCorrespondenceValue),
			min_correspondence_cost_(min_correspondence_cost),
			max_correspondence_cost_(max_correspondence_cost),
			value_to_correspondence_cost_table_(conversion_tables->GetConversionTable(
				max_correspondence_cost, min_correspondence_cost,
				max_correspondence_cost)) {
			CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
		}

		Grid2D::Grid2D(const proto::Grid2D& proto,
			ValueConversionTables* conversion_tables)
			: limits_(proto.limits()),
			correspondence_cost_cells_(),
			min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)),
			max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)),
			value_to_correspondence_cost_table_(conversion_tables->GetConversionTable(
				max_correspondence_cost_, min_correspondence_cost_,
				max_correspondence_cost_)) {
			CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
			if (proto.has_known_cells_box()) {
				const auto& box = proto.known_cells_box();
				known_cells_box_ =
					Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()),
						Eigen::Vector2i(box.max_x(), box.max_y()));
			}
			correspondence_cost_cells_.reserve(proto.cells_size());
			for (const auto& cell : proto.cells()) {
				CHECK_LE(cell, std::numeric_limits<uint16>::max());
				correspondence_cost_cells_.push_back(cell);
			}
		}

		// Finishes the update sequence.
		void Grid2D::FinishUpdate() {
			while (!update_indices_.empty()) {
				DCHECK_GE(correspondence_cost_cells_[update_indices_.back()],
					kUpdateMarker);
				correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
				update_indices_.pop_back();
			}
		}

		// Fills in 'offset' and 'limits' to define a subregion of that contains all
		// known cells.
		void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset,
			CellLimits* const limits) const {
			if (known_cells_box_.isEmpty()) {
				*offset = Eigen::Array2i::Zero();
				*limits = CellLimits(1, 1);
				return;
			}
			*offset = known_cells_box_.min().array();
			*limits = CellLimits(known_cells_box_.sizes().x() + 1,
				known_cells_box_.sizes().y() + 1);
		}

		// Grows the map as necessary to include 'point'. This changes the meaning of
		// these coordinates going forward. This method must be called immediately
		// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
		void Grid2D::GrowLimits(const Eigen::Vector2f& point) {
			GrowLimits(point, { mutable_correspondence_cost_cells() },
				{ kUnknownCorrespondenceValue });
		}

		void Grid2D::GrowLimits(const Eigen::Vector2f& point,
			const std::vector<std::vector<uint16>*>& grids,
			const std::vector<uint16>& grids_unknown_cell_values) {
			CHECK(update_indices_.empty());
			while (!limits_.Contains(limits_.GetCellIndex(point))) {
				const int x_offset = limits_.cell_limits().num_x_cells / 2;
				const int y_offset = limits_.cell_limits().num_y_cells / 2;
				const MapLimits new_limits(
					limits_.resolution(),
					limits_.max() +
					limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
					CellLimits(2 * limits_.cell_limits().num_x_cells,
						2 * limits_.cell_limits().num_y_cells));
				const int stride = new_limits.cell_limits().num_x_cells;
				const int offset = x_offset + stride * y_offset;
				const int new_size = new_limits.cell_limits().num_x_cells *
					new_limits.cell_limits().num_y_cells;

				for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {
					std::vector<uint16> new_cells(new_size,
						grids_unknown_cell_values[grid_index]);
					for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
						for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
							new_cells[offset + j + i * stride] =
								(*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells];
						}
					}
					*grids[grid_index] = new_cells;
				}
				limits_ = new_limits;
				if (!known_cells_box_.isEmpty()) {
					known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
				}
			}
		}

		proto::Grid2D Grid2D::ToProto() const {
			proto::Grid2D result;
			*result.mutable_limits() = mapping::ToProto(limits_);
			*result.mutable_cells() = { correspondence_cost_cells_.begin(),
									   correspondence_cost_cells_.end() };
			CHECK(update_indices().empty()) << "Serializing a grid during an update is "
				"not supported. Finish the update first.";
			if (!known_cells_box().isEmpty()) {
				auto* const box = result.mutable_known_cells_box();
				box->set_max_x(known_cells_box().max().x());
				box->set_max_y(known_cells_box().max().y());
				box->set_min_x(known_cells_box().min().x());
				box->set_min_y(known_cells_box().min().y());
			}
			result.set_min_correspondence_cost(min_correspondence_cost_);
			result.set_max_correspondence_cost(max_correspondence_cost_);
			return result;
		}

	}  // namespace mapping
}  // namespace cartographer
